#ifndef VECMATH_H
#define VECMATH_H

#define _USE_MATH_DEFINES
#include "math.h"

typedef float Vec2f[2];
typedef float Vec3f[3];
typedef float Vec4f[4];

typedef unsigned char byte;
typedef byte Color4b[4];

typedef Vec4f Mat4f[4];

typedef struct {
    Vec3f v;
    Color4b c;
} PackedVecColor;

float DegreesToRadians(float d);
float RadiansToDegrees(float r);

float Lerp(float start, float end, float norm);
float InvLerp(float start, float end, float norm);

void SetVec2f(Vec2f v, float x, float y);
void SetVec2f(Vec2f v, const Vec2f in);
void SetVec3f(Vec3f v, float x, float y, float z);
void SetVec3f(Vec3f v, const Vec3f in);
void SetVec3fVec4f(Vec3f v, Vec4f in);
void SetVec4f(Vec4f v, float x, float y, float z, float w);
void SetVec4f(Vec4f v, const Vec4f in);
void SetVec4fVec3f(Vec4f v, const Vec3f in);
void SetColor4b(Color4b c, unsigned char r, unsigned char g, unsigned char b, unsigned char a);
void SetColor4b(Color4b c, float r, float g, float b, float a);
void SetColor4b(Color4b c, const Vec4f components);
void SetMat4f(Mat4f out,
              float r0c0, float r0c1, float r0c2, float r0c3,
              float r1c0, float r1c1, float r1c2, float r1c3,
              float r2c0, float r2c1, float r2c2, float r2c3,
              float r3c0, float r3c1, float r3c2, float r3c3);
void SetMat4f(Mat4f out, const Vec3f bx, const Vec3f by, const Vec3f bz);

void ScaleColor4b(Color4b out, const Color4b in, float scale);

void NormalizeVec3f(Vec3f out, const Vec3f in);

float InnerProductVec3f(const Vec3f a, const Vec3f b);
float InnerProductVec3fVec4f(const Vec3f a, const Vec4f b);
void CrossProductVec3f(Vec3f out, const Vec3f a, const Vec3f b);

void ScaleVec3f(Vec3f out, const Vec3f a, float s);
void ComponentScaleVec3f(Vec3f out, const Vec3f a, const Vec3f s);

void AddVec3f(Vec3f out, const Vec3f a, const Vec3f b);
void SubtractVec3f(Vec3f out, const Vec3f a, const Vec3f b);

bool EqualVec3f(const Vec3f a, const Vec3f b);

void MultiplyRightMat4f(Mat4f out, const Mat4f right);
void MultiplyLeftMat4f(const Mat4f left, Mat4f out);
void MultiplyMat4f(Mat4f out, const Mat4f a, const Mat4f b);
void MultiplyMat4fVec4f(Vec4f out, const Mat4f m, const Vec4f v);
void MultiplyMat4fVec3f(Vec3f out, const Mat4f m, const Vec3f v);

void ViewportMat4f(Mat4f out, float width, float height);
void InverseViewportMat4f(Mat4f out, float width, float heigth);
void FrustumMat4f(Mat4f out, float l, float r, float b, float t, float n, float f);
void InverseFrustumMat4f(Mat4f out, float l, float r, float b, float t, float n, float f);
void ScaleMat4f(Mat4f out, float x, float y, float z);
void ScaleMat4f(Mat4f out, const Vec3f s);
void InverseScaleMat4f(Mat4f out, float x, float y, float z);
void InverseScaleMat4f(Mat4f out, const Vec3f s);
void TranslateMat4f(Mat4f out, float x, float y, float z);
void TranslateMat4f(Mat4f out, const Vec3f s);
void InverseTranslateMat4f(Mat4f out, float x, float y, float z);
void InverseTranslateMat4f(Mat4f out, const Vec3f s);
void RotateXMat4f(Mat4f out, float d);
void InverseRotateXMat4f(Mat4f out, float d);
void RotateYMat4f(Mat4f out, float d);
void InverseRotateYMat4f(Mat4f out, float d);
void RotateZMat4f(Mat4f out, float d);
void InverseRotateZMat4f(Mat4f out, float d);

inline float DegreesToRadians(float d) {
    return d * M_PI / 180.0f;
}

inline float RadiansToDegrees(float r) {
    return r * 180.0f / M_PI;
}

inline float Lerp(float start, float end, float norm) {
	return start + (end - start) * norm;
}

inline float InvLerp(float start, float end, float norm) {
	return end + (start - end) * norm;
}

inline void SetVec2f(Vec2f v, float x, float y) {
    v[0] = x; v[1] = y;
}

inline void SetVec2f(Vec2f v, const Vec2f in) {
    v[0] = in[0]; v[1] = in[1];
}

inline void SetVec3f(Vec3f v, float x, float y, float z) {
    v[0] = x; v[1] = y; v[2] = z;
}

inline void SetVec3f(Vec3f v, const Vec3f in) {
    v[0] = in[0];
    v[1] = in[1];
    v[2] = in[2];
}

inline void SetVec3fVec4f(Vec3f v, const Vec4f in) {
    float invW = 1.0f / in[3];
    v[0] = in[0] * invW;
    v[1] = in[1] * invW;
    v[2] = in[2] * invW;
}

inline void SetVec4f(Vec4f v, float x, float y, float z, float w) {
    v[0] = x; v[1] = y; v[2] = z; v[3] = w;
}

inline void SetVec4f(Vec4f v, const Vec4f in) {
    v[0] = in[0];
    v[1] = in[1];
    v[2] = in[2];
    v[3] = in[3];
}

inline void SetVec4fVec3f(Vec4f v, const Vec3f in) {
    v[0] = in[0];
    v[1] = in[1];
    v[2] = in[2];
    v[3] = 1.0f;
}

inline void SetColor4b(Color4b c, unsigned char r, unsigned char g, unsigned char b, unsigned char a) {
    c[0] = r; c[1] = g; c[2] = b; c[3] = a;
}

inline void SetColor4b(Color4b c, const Color4b in) {
    c[0] = in[0];
    c[1] = in[1];
    c[2] = in[2];
    c[3] = in[3];
}

inline void SetColor4b(Color4b c, float r, float g, float b, float a) {
    c[0] = 255.0f * r;
    c[1] = 255.0f * g;
    c[2] = 255.0f * b;
    c[3] = 255.0f * a;
}

inline void SetColor4b(Color4b c, const Vec4f components) {
    c[0] = 255.0f * components[0];
    c[1] = 255.0f * components[1];
    c[2] = 255.0f * components[2];
    c[3] = 255.0f * components[3];
}

inline void SetMat4f(Mat4f out,
              float r0c0, float r0c1, float r0c2, float r0c3,
              float r1c0, float r1c1, float r1c2, float r1c3,
              float r2c0, float r2c1, float r2c2, float r2c3,
              float r3c0, float r3c1, float r3c2, float r3c3) {
    out[0][0] = r0c0; out[0][1] = r0c1; out[0][2] = r0c2; out[0][3] = r0c3;
    out[1][0] = r1c0; out[1][1] = r1c1; out[1][2] = r1c2; out[1][3] = r1c3;
    out[2][0] = r2c0; out[2][1] = r2c1; out[2][2] = r2c2; out[2][3] = r2c3;
    out[3][0] = r3c0; out[3][1] = r3c1; out[3][2] = r3c2; out[3][3] = r3c3;
}

inline void SetMat4f(Mat4f out, const Vec3f bx, const Vec3f by, const Vec3f bz) {
    SetMat4f(out,
             bx[0], by[0], bz[0], 0,
             bx[1], by[1], bz[1], 0,
             bx[2], by[2], bz[2], 0,
             bx[3], by[3], bz[3], 1);
}

inline void ScaleColor4b(Color4b out, const Color4b in, float scale) {
	out[0] = (float)in[0] * scale;
	out[1] = (float)in[1] * scale;
	out[2] = (float)in[2] * scale;
	out[3] = (float)in[3] * scale;
}

inline void NormalizeVec3f(Vec3f out, const Vec3f in) {
    float invLength = 1 / sqrtf(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]);
    out[0] = in[0] * invLength;
    out[1] = in[1] * invLength;
    out[2] = in[2] * invLength;
}

inline float InnerProductVec3f(const Vec3f a, const Vec3f b) {
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

inline float InnerProductVec3fVec4f(const Vec3f a, const Vec4f b) {
	return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + b[3];
}

inline void CrossProductVec3f(Vec3f out, const Vec3f a, const Vec3f b) {
    out[0] = a[1]*b[2] - a[2]*b[1];
    out[1] = a[2]*b[0] - a[0]*b[2];
    out[2] = a[0]*b[1] - a[1]*b[0];
}

inline void ScaleVec3f(Vec3f out, const Vec3f a, float s) {
    out[0] = a[0] * s;
    out[1] = a[1] * s;
    out[2] = a[2] * s;
}

inline void ComponentScaleVec3f(Vec3f out, const Vec3f a, const Vec3f s) {
	out[0] = a[0] * s[0];
	out[1] = a[1] * s[1];
	out[2] = a[2] * s[2];
}

inline void AddVec3f(Vec3f out, const Vec3f a, const Vec3f b) {
    out[0] = a[0] + b[0];
    out[1] = a[1] + b[1];
    out[2] = a[2] + b[2];
}

inline void SubtractVec3f(Vec3f out, const Vec3f a, const Vec3f b) {
    out[0] = a[0] - b[0];
    out[1] = a[1] - b[1];
    out[2] = a[2] - b[2];
}

inline bool EqualVec3f(const Vec3f a, const Vec3f b) {
    return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]);
}

inline float DistanceVec3f(const Vec3f a, const Vec3f b) {
	Vec3f diff;
	SubtractVec3f(diff, b, a);
	return sqrtf(InnerProductVec3f(a, b));
}

inline void ViewportMat4f(Mat4f out, float width, float height) {
    SetMat4f(out,
             width / 2.0f, 0, 0, width / 2.0f,
             0, height / 2.0f, 0, height / 2.0f,
             0, 0, 1, 0,
             0, 0, 0, 1);
}

inline void InverseViewportMat4f(Mat4f out, float width, float heigth) {
    SetMat4f(out,
             2.0f / width, 0, 0, -1,
             0, 2.0f / heigth, 0, -1,
             0, 0, 1, 0,
             0, 0, 0, 1);
}

inline void FrustumMat4f(Mat4f out, float l, float r, float b, float t, float n, float f) {
    SetMat4f(out,
             2.0f*n/(r-l), 0, (r+l)/(r-l), 0,
             0, 2.0f*n/(t-b), (t+b)/(t-b), 0,
             0, 0, -(f+n)/(f-n), -2.0f*f*n/(f-n),
             0, 0, -1, 0);
}

inline void InverseFrustumMat4f(Mat4f out, float l, float r, float b, float t, float n, float f) {
    SetMat4f(out,
             (r-l)/(2.0f*n), 0, 0, (r+l)/(2.0f*n),
             0, (t-b)/(2.0f*n), 0, (t+b)/(2.0f*n),
             0, 0, 0, -1,
             0, 0, -(f-n)/(2.0f*f*n), (f+n)/(2.0f*f*n));
}

inline void ScaleMat4f(Mat4f out, float x, float y, float z) {
    SetMat4f(out,
             x, 0, 0, 0,
             0, y, 0, 0,
             0, 0, z, 0,
             0, 0, 0, 1);
}

inline void ScaleMat4f(Mat4f out, const Vec3f s) {
    ScaleMat4f(out, s[0], s[1], s[2]);
}

inline void InverseScaleMat4f(Mat4f out, float x, float y, float z) {
    ScaleMat4f(out, 1.0f / x, 1.0f / y, 1.0f / z);
}

inline void InverseScaleMat4f(Mat4f out, const Vec3f s) {
    InverseScaleMat4f(out, s[0], s[1], s[2]);
}

inline void TranslateMat4f(Mat4f out, float x, float y, float z) {
    SetMat4f(out,
             1, 0, 0, x,
             0, 1, 0, y,
             0, 0, 1, z,
             0, 0, 0, 1);
}

inline void TranslateMat4f(Mat4f out, const Vec3f s) {
    TranslateMat4f(out, s[0], s[1], s[2]);
}

inline void InverseTranslateMat4f(Mat4f out, float x, float y, float z) {
    TranslateMat4f(out, -x, -y, -z);
}

inline void InverseTranslateMat4f(Mat4f out, const Vec3f s) {
    InverseTranslateMat4f(out, s[0], s[1], s[2]);
}

inline void RotateXMat4f(Mat4f out, float d) {
    float r = DegreesToRadians(d);
    SetMat4f(out,
             1, 0, 0, 0,
             0, cosf(r), -sinf(r), 0,
             0, sinf(r), cosf(r), 0,
             0, 0, 0, 1);
}

inline void InverseRotateXMat4f(Mat4f out, float d) {
    float r = DegreesToRadians(d);
    SetMat4f(out,
             1, 0, 0, 0,
             0, cosf(r), sinf(r), 0,
             0, -sinf(r), cosf(r), 0,
             0, 0, 0, 1);
}

inline void RotateYMat4f(Mat4f out, float d) {
    float r = DegreesToRadians(d);
    SetMat4f(out,
             cosf(r), 0, sinf(r), 0,
             0, 1, 0, 0,
             -sinf(r), 0, cosf(r), 0,
             0, 0, 0, 1);
}

inline void InverseRotateYMat4f(Mat4f out, float d) {
    float r = DegreesToRadians(d);
    SetMat4f(out,
             cosf(r), 0, -sinf(r), 0,
             0, 1, 0, 0,
             sinf(r), 0, cosf(r), 0,
             0, 0, 0, 1);
}

inline void RotateZMat4f(Mat4f out, float d) {
    float r = DegreesToRadians(d);
    SetMat4f(out,
             cosf(r), -sinf(r), 0, 0,
             sinf(r), cosf(r), 0, 0,
             0, 0, 1, 0,
             0, 0, 0, 1);
}

inline void InverseRotateZMat4f(Mat4f out, float d) {
    float r = DegreesToRadians(d);
    SetMat4f(out,
             cosf(r), sinf(r), 0, 0,
             -sinf(r), cosf(r), 0, 0,
             0, 0, 1, 0,
             0, 0, 0, 1);
}

#endif // VECMATH_H
